/**
  *****************************************************************************
  * @Author       : JackyJuu
  * @Site         : https://www.geekros.com
  * @LastEditTime : 2022-03-18 17:59:09
  * @FilePath     : \Geek_Dog\hardware\robot\lite_dog\src\dog_move.c
  * @Description  : 机器狗运动相关代码
  * @Copyright (c) 2022 by GEEKROS, All Rights Reserved. 
  ******************************************************************************
*/
#include "main.h"
#include "math.h"
#include "dog_app.h"
#include "dog_move.h"
#include "lite_Dog.h"
#include "servo.h"


float Legs_Max_Data(float* Legs_Dataa)
{
	float Now_Max_Height;
	Now_Max_Height = Legs_Dataa[0];
	for(int kk = 0;kk < 4;kk++)
	{
		if(Legs_Dataa[kk] >= Now_Max_Height)
			Now_Max_Height = Legs_Dataa[kk];
	}
	
	return Now_Max_Height;
}

float Legs_Min_Data(float* Legs_Dataa)
{
	float Now_Min_Height;
	Now_Min_Height = Legs_Dataa[0];
	for(int kk = 0;kk < 4;kk++)
	{
		if(Legs_Dataa[kk] <= Now_Min_Height)
			Now_Min_Height = Legs_Dataa[kk];
	}
	
	return Now_Min_Height;
}

void Data_Limit(float* Dataa,float Max_Data,float Min_Data)
{
	if(*Dataa > Max_Data)
		*Dataa = Max_Data;
	else if(*Dataa < Min_Data)
		*Dataa = Min_Data;           
}

#define Dog_Aatt_Error 0.5

//到地
void Dog_Reback_Body_Mode()
{
	
}

float Pit_Set_Kp = 0.06;
float Roll_Set_Kp = 0.1;
float Yaw_Set_Kp = 0.1;

/*******************************************************************************
  * @funtion      : Lite_Dog_Set_Attitude_Angle  
  * @LastEditors  : JackyJuu
  * @description  : 目标姿态角度设置
  * @param         {Lite_Dog_Struct*} Dog_Set_Attitude_Angle 存储机器狗数据结构体地址
  * @param         {float} Pitch_Target 目标pitch轴角度
  * @param         {float} Roll_Target 目标roll轴角度
  * @param         {float} Yaw_Target 目标yaw轴角度
  * @return        {*} 最终数据保存到机器狗数据结构体中目标姿态相关结构体数据中
  *******************************************************************************/
void Lite_Dog_Set_Attitude_Angle(Lite_Dog_Struct* Dog_Set_Attitude_Angle,float Pitch_Target,float Roll_Target,float Yaw_Target)
{
	float Get_Pit,Get_Roll,Get_Yaw;
	float Pitch_Add_t,Roll_Add_t,Yaw_Add_t;
	float Pitch_Add,Roll_Add,Yaw_Add;

	Get_Pit = Dog_Set_Attitude_Angle->Dog_State_Data.Dog_Now_Pitch - Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_error;
	Get_Roll = Dog_Set_Attitude_Angle->Dog_State_Data.Dog_Now_Roll - Dog_Set_Attitude_Angle->Dog_Config_Data.roll_error;
	Get_Yaw = Dog_Set_Attitude_Angle->Dog_State_Data.Dog_Now_Yaw - Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_error;

    Data_Limit(&Pitch_Target,Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max);
    Data_Limit(&Roll_Target,Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max); 
	Data_Limit(&Yaw_Target,Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max); 	

	Pitch_Add_t = Pitch_Target - Get_Pit;
	Roll_Add_t = Roll_Target - Get_Roll;
	Yaw_Add_t = Yaw_Target - Get_Yaw;
	
    Data_Limit(&Pitch_Add_t,Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max);
    Data_Limit(&Roll_Add_t,Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max); 
	Data_Limit(&Yaw_Add_t,Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max); 

	if((Pitch_Add_t > -Dog_Aatt_Error) && (Pitch_Add_t < Dog_Aatt_Error))
	{
		Pitch_Add_t = 0;
	}

	if((Roll_Add_t > -Dog_Aatt_Error) && (Roll_Add_t < Dog_Aatt_Error))
	{
		Roll_Add_t = 0;
	}

	if((Yaw_Add_t > -Dog_Aatt_Error) && (Yaw_Add_t < Dog_Aatt_Error))
	{
		Yaw_Add_t = 0;
	}

	Pitch_Add = -Pitch_Add_t * Pit_Set_Kp;	
	Roll_Add = -Roll_Add_t * Roll_Set_Kp;
	Yaw_Add =  -Yaw_Add_t * Yaw_Set_Kp;

	Dog_Set_Attitude_Angle->Target_Attitude.pitch = Pitch_Add;
	Dog_Set_Attitude_Angle->Target_Attitude.roll = Roll_Add;
	Dog_Set_Attitude_Angle->Target_Attitude.yaw = Yaw_Add;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Set_Attitude_Angle_WithoutMPU  
  * @LastEditors  : JackyJuu
  * @description  : 目标姿态角度设置(采用坐标角度解算角度差，不采用实际陀螺仪数据)
  * 				区别在于，目标当前姿态的角度通过四足坐标确定，不通过陀螺仪获取
  * @param         {Lite_Dog_Struct*} Dog_Set_Attitude_Angle 存储机器狗数据结构体地址
  * @param         {float} Pitch_Target 目标pitch轴角度
  * @param         {float} Roll_Target 目标roll轴角度
  * @param         {float} Yaw_Target 目标yaw轴角度
  * @return        {*} 最终数据保存到机器狗数据结构体中目标姿态相关结构体数据中
  *******************************************************************************/
void Lite_Dog_Set_Attitude_Angle_WithoutMPU(Lite_Dog_Struct* Dog_Set_Attitude_Angle,float Pitch_Target,float Roll_Target,float Yaw_Target)
{
	float Get_Pit,Get_Roll,Get_Yaw;
	float Pitch_Add_t,Roll_Add_t,Yaw_Add_t;
	float Pitch_Add,Roll_Add,Yaw_Add;

	Get_Pit = Dog_Set_Attitude_Angle->Dog_State_Data.Dog_Now_Pitch_WOMpu;
	Get_Roll = Dog_Set_Attitude_Angle->Dog_State_Data.Dog_Now_Roll_WOMpu;
	Get_Yaw = Dog_Set_Attitude_Angle->Dog_State_Data.Dog_Now_Yaw_WOMpu;

    Data_Limit(&Pitch_Target,Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max);
    Data_Limit(&Roll_Target,Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max); 
	Data_Limit(&Yaw_Target,Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max); 	

	Pitch_Add_t = Pitch_Target - Get_Pit;
	Roll_Add_t = Roll_Target - Get_Roll;
	Yaw_Add_t = Yaw_Target - Get_Yaw;
	
    Data_Limit(&Pitch_Add_t,Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.pitch_max);
    Data_Limit(&Roll_Add_t,Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.roll_max); 
	Data_Limit(&Yaw_Add_t,Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max,-Dog_Set_Attitude_Angle->Dog_Config_Data.yaw_max); 

	if((Pitch_Add_t > -Dog_Aatt_Error) && (Pitch_Add_t < Dog_Aatt_Error))
	{
		Pitch_Add_t = 0;
	}

	if((Roll_Add_t > -Dog_Aatt_Error) && (Roll_Add_t < Dog_Aatt_Error))
	{
		Roll_Add_t = 0;
	}

	if((Yaw_Add_t > -Dog_Aatt_Error) && (Yaw_Add_t < Dog_Aatt_Error))
	{
		Yaw_Add_t = 0;
	}

	Pitch_Add = Pitch_Add_t;	
	Roll_Add = Roll_Add_t;
	Yaw_Add =  Yaw_Add_t;

	Dog_Set_Attitude_Angle->Target_Attitude.pitch = Pitch_Add;
	Dog_Set_Attitude_Angle->Target_Attitude.roll = Roll_Add;
	Dog_Set_Attitude_Angle->Target_Attitude.yaw = Yaw_Add;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Set_Attitude_XYZ  
  * @LastEditors  : JackyJuu
  * @description  : 目标姿态XYZ设置
  * @param         {Lite_Dog_Struct*} Dog_Set_Attitude_XYZ 存储机器狗数据结构体地址
  * @param         {float} X_Set 目标X轴偏移量
  * @param         {float} Y_Set 目标X轴偏移量
  * @param         {float} Z_Set 目标X轴偏移量
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Set_Attitude_XYZ(Lite_Dog_Struct* Dog_Set_Attitude_XYZ,float X_Set,float Y_Set,float Z_Set)
{
	float X_Data,Y_Data,Z_Data;
 	
	X_Data = X_Set;
	Y_Data = Y_Set;
	Z_Data = Z_Set;

	Data_Limit(&X_Data,Dog_Set_Attitude_XYZ->Dog_Config_Data.x_max,-Dog_Set_Attitude_XYZ->Dog_Config_Data.x_max);
    Data_Limit(&Y_Data,Dog_Set_Attitude_XYZ->Dog_Config_Data.y_max,-Dog_Set_Attitude_XYZ->Dog_Config_Data.y_max); 
	Data_Limit(&Z_Data,Dog_Set_Attitude_XYZ->Dog_Config_Data.z_max,-Dog_Set_Attitude_XYZ->Dog_Config_Data.z_max); 

	Dog_Set_Attitude_XYZ->Target_Attitude.x = X_Set;
	Dog_Set_Attitude_XYZ->Target_Attitude.y = Y_Set;
	Dog_Set_Attitude_XYZ->Target_Attitude.z = Z_Set;
}

/*******************************************************************************
  * @funtion      : Dog_Trot_Cycloid  
  * @LastEditors  : JackyJuu
  * @description  : Trot运动状态下Cycloid摆线步态运动相关函数
  * @param         {int} Time 当前帧
  * @param         {int*} Start_Flag 是否初次运动标志
  * @param         {Dog_Set_Move_Data_Struct*} Trot_Move_Data 机器狗运动相关数据结构体地址
  * @param         {float*} Leg_X 足端坐标x轴坐标地址
  * @param         {float*} Leg_Y 足端坐标x轴坐标地址
  * @return        {*}
  *******************************************************************************/
void Dog_Trot_Cycloid(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Trot_Move_Data,float* Leg_X,float* Leg_Y)
{
	static float Start_X[4],Start_Y[4],Last_Y[4];
	static float Set_Alpha[4];
	float Set_X_Step,Set_Y_Step,sigma;
	float zep,xep;
	static float xep_last = 0,zep_last = 0;

	if(Time == 0 )
	{
		xep_last = 0;	
		zep_last = 0;
	}	

	if(*Start_Flag == 1)
	{
		sigma = 2* PI* Time/Trot_Move_Data->Single_Leg_Move_Time;
		zep = Trot_Move_Data->Leg_Now_Move_Height * (1 - cos(sigma))/2;
		xep = Trot_Move_Data->Leg_Now_Move_Lenth/2 * (sigma - sin(sigma))/(2 * PI);

		Set_Y_Step = zep - zep_last;
		Set_X_Step = xep - xep_last;

		xep_last = xep;	
		zep_last = zep;
	}
	else
	{
		sigma = 2* PI* Time/Trot_Move_Data->Single_Leg_Move_Time;
		zep = Trot_Move_Data->Leg_Now_Move_Height * (1 - cos(sigma))/2;
		xep = Trot_Move_Data->Leg_Now_Move_Lenth * (sigma - sin(sigma))/(2 * PI);

		Set_Y_Step = zep - zep_last;
		Set_X_Step = xep - xep_last;

		xep_last = xep;	
		zep_last = zep;
	}
	if((Time >= 0) && (Time <= Trot_Move_Data->Single_Leg_Move_Time))
	{
		
		if(Trot_Move_Data->Leg_Move_Flag == 1)
		{
			if(*Start_Flag == 1)
			{
				Leg_X[0] += Set_X_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] -= Set_X_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] -= Set_X_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] += Set_X_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				
				Leg_Y[0] += Set_Y_Step;
				Leg_Y[3] += Set_Y_Step;
			}
			else
			{
				Leg_X[0] += Set_X_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] -= Set_X_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] -= Set_X_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] += Set_X_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				
				Leg_Y[0] += Set_Y_Step;
				Leg_Y[3] += Set_Y_Step;			
			}	
		}
		else if(Trot_Move_Data->Leg_Move_Flag == 2)
		{
			*Start_Flag = 0;
			Leg_X[0] -= Set_X_Step*Trot_Move_Data->Leg_L_Now_Move_State;
			Leg_X[1] += Set_X_Step*Trot_Move_Data->Leg_R_Now_Move_State;
			Leg_X[2] += Set_X_Step*Trot_Move_Data->Leg_L_Now_Move_State;
			Leg_X[3] -= Set_X_Step*Trot_Move_Data->Leg_R_Now_Move_State;
			
			Leg_Y[1] += Set_Y_Step;
			Leg_Y[2] += Set_Y_Step;
		}			
	}
}

/*******************************************************************************
  * @funtion      : Dog_Trot_Cycloid  
  * @LastEditors  : JackyJuu
  * @description  : Trot运动状态下Oval椭圆步态运动相关函数
  * @param         {int} Time 当前帧
  * @param         {int*} Start_Flag 是否初次运动标志
  * @param         {Dog_Set_Move_Data_Struct*} Trot_Move_Data 机器狗运动相关数据结构体地址
  * @param         {float*} Leg_X 足端坐标x轴坐标地址
  * @param         {float*} Leg_Y 足端坐标x轴坐标地址
  * @return        {*}
  *******************************************************************************/
void Dog_Trot_Oval(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Trot_Move_Data,float* Leg_X,float* Leg_Y)
{
	static float Start_X[4],Start_Y[4],Last_Y[4];
	static float Set_Alpha[4];
	float Set_Step;

	if(Time == 0 )
	{
		if(Trot_Move_Data->Leg_Move_Flag == 1)
		{
			Start_X[0] = Leg_X[0];
			Start_Y[0] = Leg_Y[0];
			Start_X[3] = Leg_X[3];
			Start_Y[3] = Leg_Y[3];
			Set_Alpha[0] = Start_X[0];
			Set_Alpha[3] = Start_X[3];	
			Leg_Y[0] = Move_Oval_Cal(Set_Alpha[0],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[0],Start_Y[0],Trot_Move_Data->Leg_L_Now_Move_State);		
			Leg_Y[3] = Move_Oval_Cal(Set_Alpha[3],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[3],Start_Y[3],Trot_Move_Data->Leg_R_Now_Move_State);
			Last_Y[0] = Leg_Y[3];
			Last_Y[3] = Leg_Y[3];
		}
		else if(Trot_Move_Data->Leg_Move_Flag == 2)
		{
			Start_X[1] = Leg_X[1];
			Start_Y[1] = Leg_Y[1];
			Start_X[2] = Leg_X[2];
			Start_Y[2] = Leg_Y[2];
			Set_Alpha[1] = Start_X[1];
			Set_Alpha[2] = Start_X[2];	
			Leg_Y[1] = Move_Oval_Cal(Set_Alpha[1],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[1],Start_Y[1],Trot_Move_Data->Leg_R_Now_Move_State);			
			Leg_Y[2] = Move_Oval_Cal(Set_Alpha[2],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[2],Start_Y[2],Trot_Move_Data->Leg_L_Now_Move_State);	
			Last_Y[1] = Leg_Y[1];
			Last_Y[2] = Leg_Y[2];
		}

	}
	else if((Time > 0) && (Time <= Trot_Move_Data->Single_Leg_Move_Time))
	{
		
		if(Trot_Move_Data->Leg_Move_Flag == 1)
		{
			if(*Start_Flag == 1)
			{
				Set_Step = Trot_Move_Data->Leg_Now_Move_Lenth/(2*Trot_Move_Data->Single_Leg_Move_Time);
				Leg_X[0] += Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] -= Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] -= Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] += Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				
				Set_Alpha[0] += Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Set_Alpha[3] += Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;

				Leg_Y[0] += (Move_Oval_Cal(Set_Alpha[0],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[0],Start_Y[0],Trot_Move_Data->Leg_L_Now_Move_State) - Last_Y[0]);
				Last_Y[0] = Move_Oval_Cal(Set_Alpha[0],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[0],Start_Y[0],Trot_Move_Data->Leg_L_Now_Move_State);
				Leg_Y[3] += (Move_Oval_Cal(Set_Alpha[3],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[3],Start_Y[3],Trot_Move_Data->Leg_R_Now_Move_State) - Last_Y[3]);	
				Last_Y[3] = Move_Oval_Cal(Set_Alpha[3],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/4,Start_X[3],Start_Y[3],Trot_Move_Data->Leg_R_Now_Move_State);
			}
			else
			{
				Set_Step = Trot_Move_Data->Leg_Now_Move_Lenth/(Trot_Move_Data->Single_Leg_Move_Time);
				Leg_X[0] += Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] -= Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] -= Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] += Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;

				Set_Alpha[0] += Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
				Set_Alpha[3] += Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;
				
				Leg_Y[0] += (Move_Oval_Cal(Set_Alpha[0],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Trot_Move_Data->Leg_L_Now_Move_State) - Last_Y[0]);
				Last_Y[0] = Move_Oval_Cal(Set_Alpha[0],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Trot_Move_Data->Leg_L_Now_Move_State);
				Leg_Y[3] += (Move_Oval_Cal(Set_Alpha[3],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Trot_Move_Data->Leg_R_Now_Move_State) - Last_Y[3]);	
				Last_Y[3] = Move_Oval_Cal(Set_Alpha[3],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Trot_Move_Data->Leg_R_Now_Move_State);			
			}	
		}
		else if(Trot_Move_Data->Leg_Move_Flag == 2)
		{
			*Start_Flag = 0;
			Set_Step = Trot_Move_Data->Leg_Now_Move_Lenth/(Trot_Move_Data->Single_Leg_Move_Time);
			Leg_X[0] -= Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
			Leg_X[1] += Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;
			Leg_X[2] += Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
			Leg_X[3] -= Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;

			Set_Alpha[1] += Set_Step*Trot_Move_Data->Leg_L_Now_Move_State;
			Set_Alpha[2] += Set_Step*Trot_Move_Data->Leg_R_Now_Move_State;
							
			
			Leg_Y[1] += (Move_Oval_Cal(Set_Alpha[1],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Trot_Move_Data->Leg_R_Now_Move_State) - Last_Y[1]);
			Last_Y[1] = Move_Oval_Cal(Set_Alpha[1],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Trot_Move_Data->Leg_R_Now_Move_State);
			Leg_Y[2] += (Move_Oval_Cal(Set_Alpha[2],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[2],Start_Y[2],Trot_Move_Data->Leg_L_Now_Move_State) - Last_Y[2]);	
			Last_Y[2] = Move_Oval_Cal(Set_Alpha[2],Trot_Move_Data->Leg_Now_Move_Height,Trot_Move_Data->Leg_Now_Move_Lenth/2,Start_X[2],Start_Y[2],Trot_Move_Data->Leg_L_Now_Move_State);			
		}			
	}
}

/*******************************************************************************
  * @funtion      : Dog_Trot_Step  
  * @LastEditors  : JackyJuu
  * @description  : Trot 运动状态下原地踏步
  * @param         {int} Time 当前帧
  * @param         {int*} Start_Flag 是否初次运动标志
  * @param         {Dog_Set_Move_Data_Struct*} Trot_Move_Data 机器狗运动相关数据结构体地址
  * @param         {float*} Leg_X 足端坐标x轴坐标地址
  * @param         {float*} Leg_Y 足端坐标x轴坐标地址
  * @return        {*}
  *******************************************************************************/
void Dog_Trot_Step(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Trot_Move_Data,float* Leg_X,float* Leg_Y)
{
	static float Start_X[4],Start_Y[4],Last_Y[4];

	
	if(Trot_Move_Data->Single_Leg_Move_Time%2 != 0)
	{
		Trot_Move_Data->Single_Leg_Move_Time++;
	}
	
	if((Time > 0) && (Time <= Trot_Move_Data->Single_Leg_Move_Time/2))
	{	
		if(Trot_Move_Data->Leg_Move_Flag == 1)
		{
				Leg_Y[0] += Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[3] += Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_R_Now_Move_State;
		}
		else if(Trot_Move_Data->Leg_Move_Flag == 2)
		{
				Leg_Y[1] += Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[2] += Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_R_Now_Move_State;	
		}			
	}
	else if((Time > Trot_Move_Data->Single_Leg_Move_Time/2) && (Time <= Trot_Move_Data->Single_Leg_Move_Time))
	{	
		*Start_Flag = 0;
		if(Trot_Move_Data->Leg_Move_Flag == 1)
		{
				Leg_Y[0] -= Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[3] -= Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_R_Now_Move_State;				
		}
		else if(Trot_Move_Data->Leg_Move_Flag == 2)
		{
				Leg_Y[1] -= Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[2] -= Trot_Move_Data->Leg_Now_Move_Height/(Trot_Move_Data->Single_Leg_Move_Time/2)*Trot_Move_Data->Leg_R_Now_Move_State;						
		}			
	}
}


/*******************************************************************************
  * @funtion      : Dog_Walk_Cycloid  
  * @LastEditors  : JackyJuu
  * @description  : Walk运动状态下Cycloid摆线步态运动相关函数
  * @param         {int} Time 当前帧
  * @param         {int*} Start_Flag 是否初次运动标志
  * @param         {Dog_Set_Move_Data_Struct*} Trot_Move_Data 机器狗运动相关数据结构体地址
  * @param         {float*} Leg_X 足端坐标x轴坐标地址
  * @param         {float*} Leg_Y 足端坐标x轴坐标地址
  * @return        {*}
  *******************************************************************************/
void Dog_Walk_Cycloid(int Time,int* Start_Flag,Dog_Set_Move_Data_Struct* Walk_Move_Data,float* Leg_X,float* Leg_Y)
{
	static float Start_X[4],Start_Y[4],Last_Y[4];
	static float Set_Alpha[4];
	float Set_X_Step,Set_Y_Step,sigma;
	float zep,xep;
	static float xep_last = 0,zep_last = 0;

	if(Time == 0 )
	{
		xep_last = 0;	
		zep_last = 0;
	}	

	if(*Start_Flag == 1)
	{
		sigma = 2* PI* Time/Walk_Move_Data->Single_Leg_Move_Time;
		zep = Walk_Move_Data->Leg_Now_Move_Height * (1 - cos(sigma))/2;
		xep = Walk_Move_Data->Leg_Now_Move_Lenth/2 * (sigma - sin(sigma))/(2 * PI);

		Set_Y_Step = zep - zep_last;
		Set_X_Step = xep - xep_last;

		xep_last = xep;	
		zep_last = zep;
	}
	else
	{
		sigma = 2* PI* Time/Walk_Move_Data->Single_Leg_Move_Time;
		zep = Walk_Move_Data->Leg_Now_Move_Height * (1 - cos(sigma))/2;
		xep = Walk_Move_Data->Leg_Now_Move_Lenth * (sigma - sin(sigma))/(2 * PI);

		Set_Y_Step = zep - zep_last;
		Set_X_Step = xep - xep_last;

		xep_last = xep;	
		zep_last = zep;
	}

	switch(Walk_Move_Data->Leg_Move_Flag)
	{
		//先动LF腿
		case 1:
			if(*Start_Flag == 1)
			{
				Leg_X[0] += Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State/3;
				Leg_X[2] += Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State/3;
				Leg_X[3] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State;

				Leg_Y[0] += Set_Y_Step;
			}
			else
			{
				Leg_X[0] += Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State/3;
				Leg_X[2] -= Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State/3;
				Leg_X[3] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State/3;

				Leg_Y[0] += Set_Y_Step;
			}
		break;
		//再动RB腿
		case 2:
			*Start_Flag = 0;
			Leg_X[0] -= Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State/3;
			Leg_X[1] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State/3;
			Leg_X[2] -= Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State/3;
			Leg_X[3] += Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State;

			Leg_Y[3] += Set_Y_Step;
		break;
		//再动RF腿
		case 3:
			Leg_X[0] -= Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State/3;
			Leg_X[1] += Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State;
			Leg_X[2] -= Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State/3;
			Leg_X[3] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State/3;

			Leg_Y[1] += Set_Y_Step;
		break;
		//再动LB腿
		case 4:
			Leg_X[0] -= Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State/3;
			Leg_X[1] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State/3;
			Leg_X[2] += Set_X_Step*Walk_Move_Data->Leg_L_Now_Move_State;
			Leg_X[3] -= Set_X_Step*Walk_Move_Data->Leg_R_Now_Move_State/3;

			Leg_Y[2] += Set_Y_Step;
		break;
	}


}

/*******************************************************************************
  * @funtion      : Dog_Walk_Oval  
  * @LastEditors  : JackyJuu
  * @description  : Walk运动状态下Cycloid摆线步态运动相关函数
  * 				相关文献:https://kns.cnki.net/KXReader/Detail?TIMESTAMP=637669033889013287&DBCODE=CJFD&TABLEName=CJFDLAST2021&FileName=JXCD202104012&RESULT=1&SIGN=F8v6K3CM0uRJsSLcA2Hv8NLuH%2fo%3d
  * @param         {int} Time 当前帧
  * @param         {Dog_Set_Move_Data_Struct*} Trot_Move_Data 机器狗运动相关数据结构体地址
  * @param         {float*} Leg_X 足端坐标x轴坐标地址
  * @param         {float*} Leg_Y 足端坐标x轴坐标地址
  * @return        {*}
  * @return        {*}
  *******************************************************************************/
void Dog_Walk_Oval(int Time,Dog_Set_Move_Data_Struct* Walk_Move_Data,float* Leg_X,float* Leg_Y)
{

	static float Start_X[4];
	static float Start_Y[4];
	static float Last_Y[4];

	switch(Walk_Move_Data->Leg_Move_Flag)
	{
		//先动LB腿
		case 1:
			if(Time == 0)
			{	
				Start_X[2] = Leg_X[2];
				Start_Y[2] = Leg_Y[2];
				Last_Y[2] = Start_Y[2];
			}
			else if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[2] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[2] -= (Move_Oval_Cal(Leg_X[2],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[2],Start_Y[2],Walk_Move_Data->Leg_L_Now_Move_State) - Last_Y[2]);
				Last_Y[2] = Move_Oval_Cal(Leg_X[2],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[2],Start_Y[2],Walk_Move_Data->Leg_L_Now_Move_State);
			}
		break;
		case 2:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
		//再动LF腿
		case 3:
			if(Time == 0)
			{	
				Start_X[0] = Leg_X[0];
				Start_Y[0] = Leg_Y[0];
				Last_Y[0] = Start_Y[0];
			}
			else if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[0] -= (Move_Oval_Cal(Leg_X[0],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Walk_Move_Data->Leg_L_Now_Move_State) - Last_Y[0]);
				Last_Y[0] = Move_Oval_Cal(Leg_X[0],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Walk_Move_Data->Leg_L_Now_Move_State);
			}
		break;
		case 4:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
		//再动RB腿
		case 5:
			if(Time == 0)
			{	
				Start_X[3] = Leg_X[3];
				Start_Y[3] = Leg_Y[3];
				Last_Y[3] = Start_Y[3];
			}
			else if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[3] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_Y[3] -= (Move_Oval_Cal(Leg_X[3],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Walk_Move_Data->Leg_R_Now_Move_State) - Last_Y[3]);
				Last_Y[3] = Move_Oval_Cal(Leg_X[3],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Walk_Move_Data->Leg_R_Now_Move_State);
			}
		break;
		case 6:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
		//再动RF腿
		case 7:
			if(Time == 0)
			{	
				Start_X[1] = Leg_X[1];
				Start_Y[1] = Leg_Y[1];
				Last_Y[1] = Start_Y[1];
			}
			else if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[1] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_Y[1] -= (Move_Oval_Cal(Leg_X[1],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Walk_Move_Data->Leg_R_Now_Move_State) - Last_Y[1]);
				Last_Y[1] = Move_Oval_Cal(Leg_X[1],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Walk_Move_Data->Leg_R_Now_Move_State);
			}
		break;
		case 8:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
	}
}

/*******************************************************************************
  * @funtion      : Dog_Walk_Step  
  * @LastEditors  : JackyJuu
  * @description  : Walk运动状态下原地踏步运动相关函数
  * @param         {int} Time 当前帧
  * @param         {int*} Start_Flag 是否初次运动标志
  * @param         {Dog_Set_Move_Data_Struct*} Trot_Move_Data 机器狗运动相关数据结构体地址
  * @param         {float*} Leg_X 足端坐标x轴坐标地址
  * @param         {float*} Leg_Y 足端坐标x轴坐标地址
  * @return        {*}
  *******************************************************************************/
void Dog_Walk_Step(int Time,Dog_Set_Move_Data_Struct* Walk_Move_Data,float* Leg_X,float* Leg_Y)
{

	static float Start_X[4];
	static float Start_Y[4];
	static float Last_Y[4];

	switch(Walk_Move_Data->Leg_Move_Flag)
	{
		//先动LB腿
		case 1:
			if(Time == 0)
			{	
				Start_X[2] = Leg_X[2];
				Start_Y[2] = Leg_Y[2];
				Last_Y[2] = Start_Y[2];
			}
			else if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[2] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_Y[2] -= (Move_Oval_Cal(Leg_X[2],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[2],Start_Y[2],Walk_Move_Data->Leg_R_Now_Move_State) - Last_Y[2]);
				Last_Y[2] = Move_Oval_Cal(Leg_X[2],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[2],Start_Y[2],Walk_Move_Data->Leg_R_Now_Move_State);
			}
		break;
		case 2:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=(- (Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
		//再动LF腿
		case 3:
			if(Time == 0)
			{	
				Start_X[0] = Leg_X[0];
				Start_Y[0] = Leg_Y[0];
				Last_Y[0] = Start_Y[0];

				Leg_X[0] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[0] -= (Move_Oval_Cal(Leg_X[0],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Walk_Move_Data->Leg_L_Now_Move_State) - Last_Y[0]);
				Last_Y[0] = Move_Oval_Cal(Leg_X[0],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Walk_Move_Data->Leg_L_Now_Move_State);
			}
			else if((Time > 0) && (Time < Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_Y[0] -= (Move_Oval_Cal(Leg_X[0],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Walk_Move_Data->Leg_L_Now_Move_State) - Last_Y[0]);
				Last_Y[0] = Move_Oval_Cal(Leg_X[0],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[0],Start_Y[0],Walk_Move_Data->Leg_L_Now_Move_State);
			}
		break;
		case 4:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2+(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
		//再动RB腿
		case 5:
			if(Time == 0)
			{	
				Start_X[3] = Leg_X[3];
				Start_Y[3] = Leg_Y[3];
				Last_Y[3] = Start_Y[3];

				Leg_X[3] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_Y[3] -= (Move_Oval_Cal(Leg_X[3],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Walk_Move_Data->Leg_R_Now_Move_State) - Last_Y[3]);
				Last_Y[3] = Move_Oval_Cal(Leg_X[3],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Walk_Move_Data->Leg_R_Now_Move_State);
			}
			else if((Time > 0) && (Time < Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[3] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_Y[3] -= (Move_Oval_Cal(Leg_X[3],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Walk_Move_Data->Leg_R_Now_Move_State) - Last_Y[3]);
				Last_Y[3] = Move_Oval_Cal(Leg_X[3],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[3],Start_Y[3],Walk_Move_Data->Leg_R_Now_Move_State);
			}
		break;
		case 6:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=(-Walk_Move_Data->Leg_Now_Move_Lenth/2-(Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
		//再动RF腿
		case 7:
			if(Time == 0)
			{	
				Start_X[1] = Leg_X[1];
				Start_Y[1] = Leg_Y[1];
				Last_Y[1] = Start_Y[1];

				Leg_X[1] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_Y[1] -= (Move_Oval_Cal(Leg_X[1],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Walk_Move_Data->Leg_R_Now_Move_State) - Last_Y[1]);
				Last_Y[1] = Move_Oval_Cal(Leg_X[1],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Walk_Move_Data->Leg_R_Now_Move_State);
			}
			else if((Time > 0) && (Time < Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[1] += (Walk_Move_Data->Leg_Now_Move_Lenth)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_Y[1] -= (Move_Oval_Cal(Leg_X[1],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Walk_Move_Data->Leg_R_Now_Move_State) - Last_Y[1]);
				Last_Y[1] = Move_Oval_Cal(Leg_X[1],Walk_Move_Data->Leg_Now_Move_Height,Walk_Move_Data->Leg_Now_Move_Lenth/2,Start_X[1],Start_Y[1],Walk_Move_Data->Leg_R_Now_Move_State);
			}
		break;
		case 8:
			if((Time > 0) && (Time <= Walk_Move_Data->Single_Leg_Move_Time))
			{
				Leg_X[0] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[1] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
				Leg_X[2] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_L_Now_Move_State;
				Leg_X[3] +=((Walk_Move_Data->Dog_Config_Data->body_length)/4)/(Walk_Move_Data->Single_Leg_Move_Time)*Walk_Move_Data->Leg_R_Now_Move_State;
			}
		break;
	}
}



/*******************************************************************************
  * @funtion      : Dog_Motion_Data_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗运动相关数据设置
  * @param         {Dog_Set_Move_Data_Struct*} Move_Data_Set 机器狗运动相关数据结构体地址
  * @param         {Dog_Sport_Mode_Enum} Sport_Mode_Set 机器狗运动状态
  * @param         {Dog_Move_Mode_Enum} Move_Mode_Get 机器狗运动模式
  * @param         {float} Move_Height 设置抬腿高度
  * @param         {float} Move_Lenth 设置抬腿距离
  * @param         {float} Move_Speed 设置运动速度
  * @return        {*}
  *******************************************************************************/
void Dog_Motion_Data_Set(Dog_Set_Move_Data_Struct* Move_Data_Set,Dog_Sport_Mode_Enum Sport_Mode_Set,Dog_Move_Mode_Enum Move_Mode_Get,float Move_Height,float Move_Lenth,float Move_Speed)
{
	static float Last_Speed_Set;
	Move_Data_Set->Dog_Sport_Mode = Sport_Mode_Set;
	switch(Move_Mode_Get)
	{
		case Stop_M:
			Move_Data_Set->Dog_Move_Mode = Move_Mode_Get;
			break;
		case Front_M:
			if(Move_Speed - Last_Speed_Set < 2)
			{
				Move_Data_Set->Dog_Move_Mode = Move_Mode_Get;
			}
			Move_Data_Set->Leg_L_Move_State = 1;
			Move_Data_Set->Leg_R_Move_State = 1;
			Move_Data_Set->Move_Speed_Set = Move_Speed;
			Move_Data_Set->Move_Height_Set = Move_Height;
			Move_Data_Set->Move_Lenth_Set = Move_Lenth;
			break;
		case Left_M:
			if(Move_Speed - Last_Speed_Set < 2)
			{
				Move_Data_Set->Dog_Move_Mode = Move_Mode_Get;
			}
			Move_Data_Set->Leg_L_Move_State = -1;
			Move_Data_Set->Leg_R_Move_State = 1;
			Move_Data_Set->Move_Speed_Set = Move_Speed;
			Move_Data_Set->Move_Height_Set = Move_Height*1.5f;
			Move_Data_Set->Move_Lenth_Set = Move_Lenth*1.5f;
			break;
		case Right_M:
			if(Move_Speed - Last_Speed_Set < 2)
			{
				Move_Data_Set->Dog_Move_Mode = Move_Mode_Get;
			}
			Move_Data_Set->Leg_L_Move_State = 1;
			Move_Data_Set->Leg_R_Move_State = -1;
			Move_Data_Set->Move_Speed_Set = Move_Speed;
			Move_Data_Set->Move_Height_Set = Move_Height*1.5f;
			Move_Data_Set->Move_Lenth_Set = Move_Lenth*1.5f;
			break;
		case Back_M:
			if(Move_Speed - Last_Speed_Set < 2)
			{
				Move_Data_Set->Dog_Move_Mode = Move_Mode_Get;
			}
			Move_Data_Set->Leg_L_Move_State = -1;
			Move_Data_Set->Leg_R_Move_State = -1;
			Move_Data_Set->Move_Speed_Set = Move_Speed;
			Move_Data_Set->Move_Height_Set = Move_Height;
			Move_Data_Set->Move_Lenth_Set = Move_Lenth;
			break;
		case Step_M:
			if(Move_Speed - Last_Speed_Set < 2)
			{
				Move_Data_Set->Dog_Move_Mode = Move_Mode_Get;
			}
			Move_Data_Set->Leg_L_Move_State = 1;
			Move_Data_Set->Leg_R_Move_State = 1;
			Move_Data_Set->Move_Speed_Set = Move_Speed;
			Move_Data_Set->Move_Height_Set = Move_Height;
			Move_Data_Set->Move_Lenth_Set = 0;
			break;
		case Auto_M:
		break;
	}
	Last_Speed_Set = Move_Speed;
}

/*******************************************************************************
  * @funtion      : Dog_Motion_XY_Trot_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗tror状态运动设置坐标值
  * @param         {Dog_Set_Move_Data_Struct*} Move_XY_Set 获取运动状态数据结构体地址
  * @param         {float*} Trot_X_Cood 设置足端坐标x轴地址
  * @param         {float*} Trot_Y_Cood 设置足端坐标y轴地址
  * @return        {*}
  *******************************************************************************/
void Dog_Motion_XY_Trot_Set(Dog_Set_Move_Data_Struct* Move_XY_Set,float* Trot_X_Cood,float* Trot_Y_Cood)
{
	
	if(Move_XY_Set->Motion_Count_Time > Move_XY_Set->Single_Leg_Move_Time)
	{
		
		if(Move_XY_Set->Leg_Move_Flag == 1)
		{
			Move_XY_Set->Leg_Move_Flag = 2;
			Move_XY_Set->Single_Leg_Move_Time = (1 - Move_XY_Set->Leg_Move_Duty) * Move_XY_Set->All_Leg_Move_Time;
		}
		else if(Move_XY_Set->Leg_Move_Flag == 2)
		{
			Move_XY_Set->Leg_Move_Flag = 1;
			Move_XY_Set->Single_Leg_Move_Time = Move_XY_Set->Leg_Move_Duty * Move_XY_Set->All_Leg_Move_Time;
		}
		Move_XY_Set->Motion_Count_Time = 0;
	}
	
	if((Move_XY_Set->Motion_Count_Time == 0) && (Move_XY_Set->Leg_Move_Flag == 1))
	{
		Move_XY_Set->Leg_Now_Move_Height = Move_XY_Set->Move_Height_Set;
		Move_XY_Set->Leg_Now_Move_Lenth = Move_XY_Set->Move_Lenth_Set;
		Move_XY_Set->Leg_L_Now_Move_State = Move_XY_Set->Leg_L_Move_State;
		Move_XY_Set->Leg_R_Now_Move_State = Move_XY_Set->Leg_R_Move_State;
		Move_XY_Set->All_Leg_Move_Time = Max_Time_Set/(Move_XY_Set->Move_Speed_Set)/Dog_Delay_Time_Set;
		Move_XY_Set->Single_Leg_Move_Time = Move_XY_Set->Leg_Move_Duty * Move_XY_Set->All_Leg_Move_Time;
	}
	if(Move_XY_Set->Dog_Move_Mode != Stop_M)
	{
		if(Move_XY_Set->Dog_Move_Mode == Step_M)
		{
			Dog_Trot_Step(Move_XY_Set->Motion_Count_Time,&Move_XY_Set->Move_Start_Flag,Move_XY_Set,Trot_X_Cood,Trot_Y_Cood);				
		}
		else
		{
			Dog_Trot_Cycloid(Move_XY_Set->Motion_Count_Time,&Move_XY_Set->Move_Start_Flag,Move_XY_Set,Trot_X_Cood,Trot_Y_Cood);	
		}
		Move_XY_Set->Motion_Count_Time++;	
	}
	else
	{
		Move_XY_Set->Motion_Count_Time = 0;
		Move_XY_Set->Leg_Move_Flag = 1;			
		Move_XY_Set->Move_Start_Flag = 1;	
	}
}

/*******************************************************************************
  * @funtion      : Dog_Motion_XY_Walk_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗Walk状态运动设置坐标值
  * @param         {Dog_Set_Move_Data_Struct*} Move_XY_Set 获取运动状态数据结构体地址
  * @param         {float*} Trot_X_Cood 设置足端坐标x轴地址
  * @param         {float*} Trot_Y_Cood 设置足端坐标y轴地址
  * @return        {*}
  *******************************************************************************/
void Dog_Motion_XY_Walk_Set(Dog_Set_Move_Data_Struct* Move_XY_Set,float* Trot_X_Cood,float* Trot_Y_Cood)
{
	if(Move_XY_Set->Motion_Count_Time > Move_XY_Set->Single_Leg_Move_Time)
	{
		Move_XY_Set->Leg_Move_Flag++;
		
		if(Move_XY_Set->Leg_Move_Flag == 5)
		{
			Move_XY_Set->Leg_Move_Flag = 1;
		}
		Move_XY_Set->Motion_Count_Time = 0;
	}
	
	if((Move_XY_Set->Motion_Count_Time == 0) && (Move_XY_Set->Leg_Move_Flag == 1))
	{
		Move_XY_Set->Leg_Now_Move_Height = Move_XY_Set->Move_Height_Set;
		Move_XY_Set->Leg_Now_Move_Lenth = Move_XY_Set->Move_Lenth_Set;
		Move_XY_Set->Leg_L_Now_Move_State = Move_XY_Set->Leg_L_Move_State;
		Move_XY_Set->Leg_R_Now_Move_State = Move_XY_Set->Leg_R_Move_State;
		Move_XY_Set->All_Leg_Move_Time = Max_Time_Set/(Move_XY_Set->Move_Speed_Set)/Dog_Delay_Time_Set;
		Move_XY_Set->Single_Leg_Move_Time = Move_XY_Set->Leg_Move_Duty * Move_XY_Set->All_Leg_Move_Time;	
	}
	if(Move_XY_Set->Dog_Move_Mode != Stop_M)
	{
		if(Move_XY_Set->Dog_Move_Mode == Step_M)
		{
	//		Dog_Trot_Step(Motion_Count_Time,&Move_Start_Flag,Move_XY_Set,Trot_X_Cood,Trot_Y_Cood);				
		}
		else
		{
			Dog_Walk_Cycloid(Move_XY_Set->Motion_Count_Time,&Move_XY_Set->Move_Start_Flag,Move_XY_Set,Trot_X_Cood,Trot_Y_Cood);	
		}
		Move_XY_Set->Motion_Count_Time++;	
	}
	else
	{
		Move_XY_Set->Motion_Count_Time = 0;
		Move_XY_Set->Leg_Move_Flag = 1;	
		Move_XY_Set->Move_Start_Flag = 1;			
	}
}

/*******************************************************************************
  * @funtion      : Dog_Motion_XY_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗运动状态设置相关函数，根据运动状态调用相关的运动函数
  * @param         {Dog_Set_Move_Data_Struct*} Move_XY_Set 机器狗运动控制相关数据结构体地址
  * @param         {float*} Move_X_Cood 设置足端坐标x轴地址
  * @param         {float*} Move_Y_Cood 设置足端坐标y轴地址
  * @return        {*}
  *******************************************************************************/
void Dog_Motion_XY_Set(Dog_Set_Move_Data_Struct* Move_XY_Set,float* Move_X_Cood,float* Move_Y_Cood)
{
	static int Motion_Count_Time = 0;
	static float Leg_Now_Move_Height,Leg_Now_Move_Lenth;
	static float Leg_L_Now_Move_State,Leg_R_Now_Move_State;
	switch(Move_XY_Set->Dog_Sport_Mode)
	{
		case Trot_M:
			Move_XY_Set->Leg_Move_Duty = 0.5;
			Dog_Motion_XY_Trot_Set(Move_XY_Set,Move_X_Cood,Move_Y_Cood);
		break;
		case Walk_M:
			Move_XY_Set->Leg_Move_Duty = 0.25;
			Dog_Motion_XY_Walk_Set(Move_XY_Set,Move_X_Cood,Move_Y_Cood);
		break;	
		case Pronk_M:
			Dog_Motion_XY_Walk_Set(Move_XY_Set,Move_X_Cood,Move_Y_Cood);
		break;	
		case Stable_M:
			Dog_Motion_XY_Walk_Set(Move_XY_Set,Move_X_Cood,Move_Y_Cood);
		break;	
		case Cailbration_M:
			Dog_Motion_XY_Walk_Set(Move_XY_Set,Move_X_Cood,Move_Y_Cood);
		break;	
	}
}

